/**
* @file	: door.c
* @brief	: Contains implementation of the door handles
* @version	: 0.1
* @date	: 23. April. 2010
* @author	: Open Doors Team C4-2
**************************************************************************
*
**********************************************************************/

#include "lpc17xx.h"
#include "core_cm3.h"
#include "system_lpc17xx.h"
#include "lpc_types.h"
#include "lpc17xx_pinsel.h"
#include "lpc17xx_timer.h"
#include "lpc17xx_adc.h"
#include "lpc17xx_gpio.h"

#include "door.h"
#include "task_executive.h" // strong coupling
#include "sw_faults.h"
#include "ext_stream.h"
#include "keyboard.h"

static uint32_t voltage = 0;

//super test defines
int16_t auxiliary_buffer[256];
int16_t auxiliary_index=0;



int16_t state_buffer[1024];
int16_t state_buffer_index=0;



///

extern uint16_t x0=5000,x1=5000,x2=5000,x3=5000;



//duration_buf_t hall1duration;
//duration_buf_t hall2duration;
static duration_buf_t hall12duration;
#define X_L 128
volatile uint16_t x[X_L];
volatile int16_t x_index = 0;

static uint32_t counter_to_run = 0;
static uint32_t counter_to_enable = 0;
static uint32_t test_counter = 0;

static uint32_t TIMEOUT_afisare=0;
static uint32_t TIMEOUT_motor_run_state_4000ms =0;
static uint32_t TIMEOUT_motor_to_run_500ms = 0;
static uint32_t TIMEOUT_motor_to_stop_200ms = 0;
static FlagStatus error_motor_timeout_to_run = RESET;
static FlagStatus error_motor_timeout_to_stop = RESET;
static FlagStatus error_motor_timeout_run_state=RESET;
static FlagStatus error_force_event=RESET;



static uint32_t tt[500];
static uint32_t ttindex = 0;
#define COUNTER_TO_RUN_RELOAD 500
static FlagStatus buff_not_empty = RESET;

static uint16_t toggle_dbgpin = 0;

static MotorState motorState = MOTOR_STOP;
static FlagStatus motorBlock = RESET;



static FlagStatus motorError = RESET;

FlagStatus button_up_posedge = RESET;

//FlagStatus button_up_negedge = RESET;
FlagStatus button_dn_posedge = RESET;
//FlagStatus button_dn_negedge = RESET;

FlagStatus direction = SET; // SET --> UP

void append_duration(duration_buf_t *obj, uint16_t duration)
{
  if (duration > DEFAULT_DURATION)
  {
    duration = DEFAULT_DURATION;
  }
  obj->buf[obj->index] = duration;
  obj->index++;
  if (obj->index >= DURATION_BUF_L)
  {
    obj->index = 0;
  }
  obj->newEdge = SET;
}

static uint16_t diff1(void)
{
  int16_t i0 = hall12duration.index - 1;
  if (i0 < 0)
  {
    i0 += DURATION_BUF_L;
  }
  int16_t i1 = hall12duration.index - 9;
  if (i1 < 0)
  {
    i1 += DURATION_BUF_L;
  }
  return ((int32_t)((hall12duration.buf[i0] - hall12duration.buf[i1])) + DERIVATIVE_OFFSET_0_8);
}

static int32_t derivative(uint16_t offset)
{
  int16_t current = x_index - 1;
  if (current < 0)
  {
    current += X_L;
  }
  int16_t past =x_index - 1 - offset;
  if (past < 0)
  {
    past += X_L;
  }
  return (((int32_t)(x[(uint16_t)current])) - ((int32_t)(x[(uint16_t)past])));
}

static uint16_t diff2(void)
{
  int16_t i0 = hall12duration.index - 1;
  if (i0 < 0)
  {
    i0 += DURATION_BUF_L;
  }
  int16_t i2 = hall12duration.index - 5;
  if (i2 < 0)
  {
    i2 += DURATION_BUF_L;
  }
  int16_t i4 = hall12duration.index - 5;
  if (i4 < 0)
  {
    i4 += DURATION_BUF_L;
  }
  int16_t i5 = hall12duration.index - 6;
  if (i5 < 0)
  {
    i5 += DURATION_BUF_L;
  }
  return ((int32_t)((hall12duration.buf[i0] - hall12duration.buf[i2])) + 6000);
//  return ((int32_t)(2*((hall12duration.buf[i0] + hall12duration.buf[i2]) -
//                       (hall12duration.buf[i4] + hall12duration.buf[i5]))) + 6000);
}


/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Getter for current timer value
* 					- Returns the value stored in the TimerCounter1 Register
*
* @param[in]	None
* @return 		uint32_t
**********************************************************************/
static uint32_t Hall_timerValue(void)
{
  return LPC_TIM1->TC;
}

/*/#define HALL_TIM_FREQ ((uint32_t)250000)*/

/**  Hall 1 macro **/
#define HALL1 0
/**  Hall 2 macro **/
#define HALL2 1

static void Hall1(edge_t edge);
static void Hall2(edge_t edge);


static FlagStatus next_Hall_edge[2/* Hall1, Hall2 */][2/* falling, rising */];
/**  Flagstatus array for the Hall sensors next state */


/* Static Functions ----------------------------------------------------------- */
/** @addtogroup Door_Static_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Setter for the next state of the Hall sensors
* 				- Sets all the fields ot the next_Hall_edge array to the given value
*
* @param[in]	value instance of FlagStatus
* @return 		None
**********************************************************************/

static void set_next_Hall_edges(FlagStatus value)
{
  next_Hall_edge[HALL1][EDGE_FALL] = value;
  next_Hall_edge[HALL1][EDGE_RISE] = value;
  next_Hall_edge[HALL2][EDGE_RISE] = value;
  next_Hall_edge[HALL2][EDGE_RISE] = value;
}

/* Static Functions ----------------------------------------------------------- */
/** @addtogroup Door_Static_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Discrete setter for the next state of a single Hall sensor
* 					- Resets the states of all the Hall sensors, EXCEPT the one specified by the parameter 'Hall'
*
* @param[in]	Hall instance of uint16_t
* @param[in]   edge instance of uint16_t
* @return 		None
**********************************************************************/
static void set_next_Hall_edge(uint16_t Hall, uint16_t edge)
{
  set_next_Hall_edges(RESET);
  next_Hall_edge[Hall][edge] = SET;
}

/* Static Functions ----------------------------------------------------------- */
/** @addtogroup Door_Static_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Getter for the next state of the sensor specified by 'Hall', on the corresponding edge
*              -Returns the next state of the 'Hall' Hall with respect to the edge
*              -Actually verifies that the 'Hall' Hall sensor is in the expected edge state
*
* @param[in]	Hall instance of uint16_t
* @param[in]   edge instance of uint16_t
* @return 		FlagStatus
**********************************************************************/
static FlagStatus check_Hall_edge(uint16_t Hall, uint16_t edge)
{
  if (SET == next_Hall_edge[Hall][edge])
  {
    return SET;
  }
  else
  {
    return RESET;
  }
}

/*
input: current Hall edge
output: modifies the next Hall edge
*/

//trebuie implementat respectand tranzitia sus/jos sau jos/sus... de vazut

static void compute_next_Hall_edge(uint16_t Hall, uint16_t edge)
{
  uint16_t next_Hall = HALL1;
  uint16_t next_edge = EDGE_FALL;
  if (HALL1 == Hall)
  {
    if (EDGE_FALL == edge)
    {
      next_Hall = HALL2;
      next_edge = EDGE_FALL;
    }
    else
    {
      next_Hall = HALL2;
      next_edge = EDGE_RISE;
    }
  }
  else /* HALL2 */
  {
    if (EDGE_FALL == edge)
    {
      next_Hall = HALL1;
      next_edge = EDGE_RISE;
    }
    else
    {
      next_Hall = HALL1;
      next_edge = EDGE_FALL;
    }
  }
  set_next_Hall_edge(next_Hall, next_edge);
}

// primary data structures (ADC samples)
/**  Acquisition buffer size **/
#define ACQ_BUF_L 32

/**  Shifting value / divides by 32  / Length power**/
#define ACQ_BUF_LP 5


typedef struct
{
  uint16_t buf[ACQ_BUF_L];
  uint16_t index;
} acq_t;

static acq_t acq[2];
static uint16_t wr = 0;

// secondary data structures (filtered samples)
static uint16_t I;

static void ctrlpin_updn(FlagStatus value);
static void ctrlpin_onoff(FlagStatus value);
typedef enum {
  CTRL_OFF,
  CTRL_PRE_UP,
  CTRL_UP,
  CTRL_PRE_DN,
  CTRL_DN
} CTRL_STATE;
static CTRL_STATE ctrl_state = CTRL_OFF;

static uint32_t pre_state_downcounter = 0;
/**  PRE_STATE duration macro in number of DEC operations**/
#define PRE_STATE_DURATION 150
/**  Macro for the UP/DOWN control pin number **/
#define CTRLPINNUM_UPDN 9
/**  Macro for the GLOBAL ON/OFF control pin number **/
#define CTRLPINNUM_ONOFF 10

static FlagStatus ctrlpin_onoff_value(void);
static FlagStatus ctrlpin_updn_value(void);

static void reset_duration_buf(void)
{

  for (int i=0;i<X_L;i++){
   x[i]=DEFAULT_DURATION1;
  }

  x_index=0;

  hall12duration.lock = RESET;
  hall12duration.index = 0;
  uint16_t i;
  for (i = 0; i < DURATION_BUF_L; i++)
  {
    hall12duration.buf[i] = DEFAULT_DURATION1;
  }
  hall12duration.newEdge = RESET;




}

//static uint16_t adc_value;

/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Initializes all peripheral resources required by the door handlers
*              -Initializes the ADC input pin on P0[23] with the required settings
*              -Initializes the ADC peripheral, om channel 0, start conversion on external trigger, 5MHz clock
*              -Initializes the TIMER0 peripheral, with toggle on external pin at 200Hz
*              -Initializes the control pins UP/DOWN and ON/OFF linked to hard/soft buttons
*              -Resets aqcuisition buffers
*              -Configures four input pins . Each is sensitive to rising or falling edges and will serve as external interrupt sources
*              -The pins are hardwired two by two, a pair for each sensor P2[10,11,12,13]
*              -Enables external interrupts with maximum priority
*
* @param[in]	None
* @return 		None
**********************************************************************/

void addstate(uint8_t stare){
 state_buffer_index++;

 if (state_buffer_index == 256) state_buffer_index=0;

 state_buffer[state_buffer_index]=stare;
}



void door__init(void)
{
  for (int i=0;i<1024;i++){
   state_buffer[i]=-1;
  }





  ctrlpin_updn(RESET);
  ctrlpin_onoff(RESET);

  //adc init
  PINSEL_CFG_Type PinCfg;

  /* Configure the NVIC Preemption Priority Bits:
  * two (2) bits of preemption priority, six (6) bits of sub-priority.
  * Since the Number of Bits used for Priority Levels is five (5), so the
  * actual bit number of sub-priority is three (3)
  */
  //NVIC_SetPriorityGrouping(0x05);

  /*
  * Init ADC pin connect
  * AD0.2 on P0.25
  */

  PinCfg.Funcnum = PINSEL_FUNC_1;
  PinCfg.OpenDrain = PINSEL_PINMODE_NORMAL;
  PinCfg.Pinmode = PINSEL_PINMODE_PULLUP;
  PinCfg.Pinnum = PINSEL_PIN_25;
  PinCfg.Portnum = PINSEL_PORT_0;
  PINSEL_ConfigPin(&PinCfg);

  /* Configuration for ADC :
  * 	Frequency at 5Mhz
  *  ADC channel 0, generate Interrupt when ADC channel 0 is converted completely
  */
  ADC_Init(LPC_ADC, 1000000);
  ADC_IntConfig(LPC_ADC,ADC_ADINTEN2,ENABLE);
  ADC_ChannelCmd(LPC_ADC,ADC_CHANNEL_2,ENABLE);

  /* preemption = 1, sub-priority = 1 */
  //NVIC_SetPriority(ADC_IRQn, ((0x01<<3)|0x01));

  NVIC_EnableIRQ(ADC_IRQn);

  // Hardware-triggered conversion
  // BURST = 0
  // START = ADC_START_ON_MAT01
  ADC_StartCmd(LPC_ADC,ADC_START_ON_MAT01);

  // timer init
  TIM_TIMERCFG_Type TIM_ConfigStruct;
  TIM_MATCHCFG_Type TIM_MatchConfigStruct;

  // Initialize timer 0, prescale count time of 1us
  TIM_ConfigStruct.PrescaleOption = TIM_PRESCALE_USVAL;
  TIM_ConfigStruct.PrescaleValue = 1;
  // use channel 1, MR1
  TIM_MatchConfigStruct.MatchChannel = 1;
  // Enable interrupt when MR0 matches the value in TC register
  TIM_MatchConfigStruct.IntOnMatch = TRUE;
  // Enable reset on MR1: TIMER will reset if MR1 matches it
  TIM_MatchConfigStruct.ResetOnMatch = TRUE;
  // Stop on MR0 if MR1 matches it
  TIM_MatchConfigStruct.StopOnMatch = FALSE;
  // What to do when with MR1.0 pin if MR1 matches it
  TIM_MatchConfigStruct.ExtMatchOutputType = TIM_EXTMATCH_TOGGLE;
  // Set Match value, count value of 5000 (5000 * 1us = 5000us --> 200 Hz)
  TIM_MatchConfigStruct.MatchValue = 5000;

  // Set configuration for Tim_config and Tim_MatchConfig
  TIM_Init(LPC_TIM0, TIM_TIMER_MODE,&TIM_ConfigStruct);
  TIM_ConfigMatch(LPC_TIM0,&TIM_MatchConfigStruct);
  //  /* Enable interrupt for timer 0 */
  //  NVIC_EnableIRQ(TIMER0_IRQn);
  // To start timer 0
  TIM_Cmd(LPC_TIM0,ENABLE);

  // reset acquisition buffers
  acq[0].index = 0;
  acq[1].index = 0;
  wr = 0;

  /* Configure control pins */
  /* UP/DOWN pin = P1.9 */
  /* ON/OFF  pin = P1.10 */
  PinCfg.Funcnum = PINSEL_FUNC_0;
  PinCfg.OpenDrain = PINSEL_PINMODE_NORMAL;
  PinCfg.Pinmode = PINSEL_PINMODE_PULLDOWN;
  PinCfg.Portnum = 1;
  PinCfg.Pinnum = CTRLPINNUM_UPDN;
  PINSEL_ConfigPin(&PinCfg);
  PinCfg.Pinnum = CTRLPINNUM_ONOFF;
  PINSEL_ConfigPin(&PinCfg);
  GPIO_SetDir(1, (1 << CTRLPINNUM_UPDN) | (1 << CTRLPINNUM_ONOFF), 1);


  /* Configure external interrupt pins */
  /* Jumpers:
  J37 = 2-3 to select INT1
  */
  //Initialize EXT registers
  LPC_SC->EXTINT = 0x0;
  LPC_SC->EXTMODE = 0x0;
  LPC_SC->EXTPOLAR = 0x0;

  /* edge sensitive */
  LPC_SC->EXTMODE = 0xF; /* toate 4 sunt sensibile pe front */
  /* falling/rising-edge sensitive */
  LPC_SC->EXTPOLAR = 0xC; /* 2 asa si 2 invers */
  /* External Interrupt Flag cleared*/
  LPC_SC->EXTINT = 0xF;

  /* P2.10 as /EINT0 */
  PinCfg.Funcnum = 1;
  PinCfg.OpenDrain = 0;
  PinCfg.Pinmode = 0;
  PinCfg.Portnum = 2;
  PinCfg.Pinnum = 10;
  PINSEL_ConfigPin(&PinCfg);
  /* P2.11 as /EINT1 */
  PinCfg.Pinnum = 11;
  PINSEL_ConfigPin(&PinCfg);
  /* P2.12 as /EINT2 */
  PinCfg.Pinnum = 12;
  PINSEL_ConfigPin(&PinCfg);
  /* P2.13 as /EINT3 */
  PinCfg.Pinnum = 13;
  PINSEL_ConfigPin(&PinCfg);

  // Enable GPIO interrupt  P0.25/AD0.2
  ///? LPC_GPIOINT->IO0IntEnF = 0x02000000;

  // Hall timer init
  // Initialize timer 1, prescale count time of 1us
  TIM_ConfigStruct.PrescaleOption = TIM_PRESCALE_USVAL;
  TIM_ConfigStruct.PrescaleValue = 1;

  // Set configuration for Tim_config and Tim_MatchConfig
  TIM_Init(LPC_TIM1, TIM_TIMER_MODE,&TIM_ConfigStruct);
  //TIM_ConfigMatch(LPC_TIM0,&TIM_MatchConfigStruct);
  //  /* Enable interrupt for timer 0 */
  //  NVIC_EnableIRQ(TIMER0_IRQn);
  // To start timer 1
  TIM_Cmd(LPC_TIM1,ENABLE);

  NVIC_SetPriority(EINT0_IRQn, 0);
  NVIC_SetPriority(EINT1_IRQn, 0);
  NVIC_SetPriority(EINT2_IRQn, 0);
  NVIC_SetPriority(EINT3_IRQn, 0);
  NVIC_EnableIRQ(EINT0_IRQn);
  NVIC_EnableIRQ(EINT1_IRQn);
  NVIC_EnableIRQ(EINT2_IRQn);
  NVIC_EnableIRQ(EINT3_IRQn);

  // initialize next edge to expect
  set_next_Hall_edges(SET);

  // initialize hall duration buffers
  reset_duration_buf();
}

// vezi sa trimiti si o datagrama de sincronizare periodic
// si sa ai grija sa nu depasesti latimea de banda


/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Contains all the logic for window control and monitoring
*              -Sends a synchronise datagram to the monitor program via USART
*              -Resets all command button flags
*              -Verifies that the transitions are valid from previous values to current values, updates the previous values correspondingly
*              -Executes the control state-machine logic
*              -Toggles between the following states: CTRL_OFF, CTRL_PRE_UP, CTRL_PRE_DOWN, CTRL_UP, CTRL_DOWN and default, which generates a fault
*              -Verified call frequency of 1KHz ~> 1ms
*
* @param[in]	None
* @return 		None
**********************************************************************/

void door__run(void)
{
  //dbgpin(SET);

  uint16_t i;
  // verify if any of the aquision buffers is full
  for (i = 0; i < 2; i++)
  {
    if (acq[i].index >= ACQ_BUF_L)
    {
      // is full
      // compute average
      uint16_t j;
      uint32_t sum = 0;
      for (j = 0; j < ACQ_BUF_L; j++)
      {
        sum += acq[i].buf[j];
      }
      // now it's time to reset the buffer index <--> empty buffer
      acq[i].index = 0;
      const uint16_t avg = sum >> ACQ_BUF_LP;
      I = avg; // trebuie modificat din curent in temperatura...

      // transmit sync
      Datagram datagram;
      datagram.size = 0; // reset datagram
      AppendChar(&datagram, '$');
      ext_stream__write(&datagram);

      // transmit I
      datagram.size = 0; // reset datagram
      AppendText(&datagram, "n1", 2);
      voltage = (float)I/(float)175 + 0.5f;
      AppendValue(&datagram, voltage); // 170 de unitati pe volt
      ext_stream__write(&datagram);
    }
  }

  // execute state machine

  // compute edge transition events
  if (SET == key_value(KEY_UP) && RESET == key_prev_value(KEY_UP))
  {
    button_up_posedge = SET;
  }
  //  if (RESET == key_value(KEY_UP) && SET == key_prev_value(KEY_UP))
  //  {
  //    button_up_negedge = SET;
  //  }
  if (SET == key_value(KEY_DN) && RESET == key_prev_value(KEY_DN))
  {
    button_dn_posedge = SET;
  }
  //  if (RESET == key_value(KEY_DN) && SET == key_prev_value(KEY_DN))
  //  {
  //    button_dn_negedge = SET;
  //  }

  update_prev_keys();

  // execute state machine transitions
//  CTRL_STATE next_ctrl_state = ctrl_state;
//  switch (ctrl_state)
//  {
//  case CTRL_OFF:
//    {
//      // if (RESET == motor_overload)
//      {
//        if (SET == button_up_posedge)
//        {
//          // go to PRE UP
//          next_ctrl_state = CTRL_PRE_UP;
//          pre_state_downcounter = PRE_STATE_DURATION;
//          ctrlpin_updn(SET);
//          break;
//        }
//        if (SET == button_dn_posedge)
//        {
//          // go to PRE DOWN
//          next_ctrl_state = CTRL_PRE_DN;
//          pre_state_downcounter = PRE_STATE_DURATION;
//          ctrlpin_updn(RESET);
//          break;
//        }
//      }
//      motor_state = OFF;
//      // stay OFF
//      //ctrlpin_onoff(RESET);
//      //ctrlpin_updn(RESET);
//    }
//    break;
//  case CTRL_PRE_UP:
//    {
//      if (RESET == key_value(KEY_UP))
//      {
//        // go to OFF
//        next_ctrl_state = CTRL_OFF;
//        ctrlpin_onoff(RESET);
//      }
//      else
//      {
//        if (0 == pre_state_downcounter)
//        {
//          // go to UP
//          next_ctrl_state = CTRL_UP;
//          ctrlpin_onoff(SET);
//          motor_state = ON;
//        }
//        else
//        {
//          pre_state_downcounter--;
//        }
//      }
//    }
//    break;
//  case CTRL_UP:
//    {
//      if (RESET == key_value(KEY_UP) || OFF == motor_state)
//      {
//        // go to OFF
//        next_ctrl_state = CTRL_OFF;
//        ctrlpin_onoff(RESET);
//      }
//      else
//      {
////        const int32_t d = derivative(8);
////        if (d > DELTA_TO || d < -DELTA_0_8 && (transition_counter))
////        {
////          motor_state = OFF;
////        }
//      }
//    }
//    break;
//  case CTRL_PRE_DN:
//    {
//      if (RESET == key_value(KEY_DN))
//      {
//        // go to OFF
//        next_ctrl_state = CTRL_OFF;
//        ctrlpin_onoff(RESET);
//      }
//      else
//      {
//        if (0 == pre_state_downcounter)
//        {
//          // go to DOWN
//          next_ctrl_state = CTRL_DN;
//          ctrlpin_onoff(SET);
//        }
//        else
//        {
//          pre_state_downcounter--;
//        }
//      }
//    }
//    break;
//  case CTRL_DN:
//    {
//      if (RESET == key_value(KEY_DN))
//      {
//        // go to OFF
//        next_ctrl_state = CTRL_OFF;
//        ctrlpin_onoff(RESET);
//      }
//      else
//      {
//      }
//    }
//    break;
//  default:
//    {
//      ctrlpin_onoff(RESET);
//      post_sw_fault("Invalid ctrl state.");
//    }
//  }
//
//  // update next
//  ctrl_state = next_ctrl_state;
  TIMEOUT_afisare++;
  counter_to_run++;
  counter_to_enable++;
  TIMEOUT_motor_to_run_500ms++;
  TIMEOUT_motor_to_stop_200ms++;
  TIMEOUT_motor_run_state_4000ms++;
  runMotorStateMachine();


  //dbgpin(RESET);
}

/*----------------------------------------------------------------------------*/

/*
S-a verificat faptul ca aceasta intrerupere se invoca la 20KHz.
*/


/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Clear Interrupt Pending for TIMER0
* 					- Clears the interrupt pending flag
*                  - Verified invoke frequency of 20KHz
*
* @param[in]	None
* @return 		None
**********************************************************************/


void TIMER0_IRQHandler(void)
{
  //dbgpin(SET);
  if (TIM_GetIntStatus(LPC_TIM0,1))
  {
    TIM_ClearIntPending(LPC_TIM0,1);
    /* to do */
  }
  //dbgpin(RESET);
}

/*
S-a verificat faptul ca aceasta intrerupere se invoca la 10KHz.
*/


/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/
/*********************************************************************//**
* @brief 		ADC interrupt handler
* 					- Get ADC value if channel is validated
* 					- Check if the acquisiton buffer is not full
* 					- Store acquired value into the buffer
*                  - If index exceeds maximum length, change buffer
*                  - Clear interrupt pending on ADC, otherwise the interrupt handler will be called upon again
*                  - Verified invoke frequency of 10KHz
*
* @param[in]	None
* @return 		None
**********************************************************************/

void ADC_IRQHandler(void)
{
  //dbgpin(SET);
  if (ADC_ChannelGetStatus(LPC_ADC,ADC_CHANNEL_2,ADC_DATA_DONE))
  {
    // retrieve
    const uint16_t adc_value = ADC_ChannelGetData(LPC_ADC,ADC_CHANNEL_2);

    // check buffer
    if (acq[wr].index >= ACQ_BUF_L)
    {
      post_sw_fault("acq buf ovf");
    }

    // store
    acq[wr].buf[acq[wr].index] = adc_value;

    // move index
    acq[wr].index++;
    if (acq[wr].index >= ACQ_BUF_L)
    {
      // full buffer
      // change write head
      switch (wr)
      {
      case 0: wr = 1; break;
      case 1: wr = 0; break;
      default: post_sw_fault("bad wr head");
      }
    }
  }
  /* Call this function, otherwise the IRQ will be called again and again */
  ADC_GlobalGetStatus(LPC_ADC,1);
  NVIC_ClearPendingIRQ(ADC_IRQn);
  //dbgpin(RESET);
}

/*
S-a verificat faptul ca intreruperea se executa dupa cum se intentioneaza.
*/

/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Capture Hall 1 FALLING edge handler
* 					- Clear the interrupt flag, ready for another capture
* 					- Call "Hall1(EDGE_FALL)"
*
* @param[in]	None
* @return 		None
**********************************************************************/


void EINT0_IRQHandler(void)
{
  //dbgpin(SET);

  LPC_SC->EXTINT |= (1     );  //clear the EINT0 flag
  hall12duration.lock = SET;
  Hall1(EDGE_FALL);
  hall12duration.lock = RESET;

  //dbgpin(RESET);
}

/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Capture Hall 2 FALLING edge handler
* 					- Clear the interrupt flag, ready for another capture
* 					- Call "Hall2(EDGE_FALL)"
*
* @param[in]	None
* @return 		None
**********************************************************************/

void EINT1_IRQHandler(void)
{
  //dbgpin(SET);
  LPC_SC->EXTINT |= (1 << 1);  //clear the EINT1 flag
  hall12duration.lock = SET;
  Hall2(EDGE_FALL);
  hall12duration.lock = RESET;
 //dbgpin(RESET);
}

/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Capture Hall 1 RISING edge handler
* 					- Clear the interrupt flag, ready for another capture
* 					- Call "Hall1(EDGE_RISE)"
*
* @param[in]	None
* @return 		None
**********************************************************************/

void EINT2_IRQHandler(void)
{
  //dbgpin(SET);
  LPC_SC->EXTINT |= (1 << 2);  //clear the EINT2 flag
  hall12duration.lock = SET;
  Hall1(EDGE_RISE);
  hall12duration.lock = RESET;
  //dbgpin(RESET);
}
/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Public_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Capture Hall 2 RISING edge handler
* 					- Clear the interrupt flag, ready for another capture
* 					- Call "Hall2(EDGE_RISE)"
*
* @param[in]	None
* @return 		None
**********************************************************************/

void EINT3_IRQHandler(void)
{
  //dbgpin(SET);
  LPC_SC->EXTINT |= (1 << 3);  //clear the EINT3 flag
  hall12duration.lock = SET;
  Hall2(EDGE_RISE);
  hall12duration.lock = RESET;
  //dbgpin(RESET);
}

/*----------------------------------------------------------------------------*/

static void ctrlpin_updn(FlagStatus value)
{
  const uint32_t mask = (1 << CTRLPINNUM_UPDN);
  if (SET == value)
  {
    LPC_GPIO1->FIOSET = mask;
  }
  else
  {
    LPC_GPIO1->FIOCLR = mask;
  }
  direction = value;
}

static void ctrlpin_onoff(FlagStatus value)
{
  const uint32_t mask = (1 << CTRLPINNUM_ONOFF);
  if (RESET/* active on low */ == value)
  {
    // dezactivare
    //ctrlpin_onoff = SET;
    LPC_GPIO1->FIOSET = mask;
    // cand il inactivezi: tine minte in ce directie s-a miscat geamul
    //prev_motor_direction_was_up = ctrlpin_updn_value();
  }
  else
  {
    // activare
    // nu se poate activa doar daca directia precedenta a fost inversa
    //if (ctrlpin_updn_value() == (!prev_motor_direction_was_up))
    {
      //ctrlpin_onoff = RESET;
      LPC_GPIO1->FIOCLR = mask;
      //TIM_ResetCounter(LPC_TIM1);
      //reset_duration_buf();
    }
    //else
    {
      // ce faci cand este aceeasi directie
    }
  }
}

/** Tells the state of the control pin UP/DOWN */
/* SET --> UP, RESET --> DOWN */
static FlagStatus ctrlpin_updn_value(void)
{
  const uint32_t mask = (1 << CTRLPINNUM_UPDN);
  if (GPIO_ReadValue(1) & mask)
  {
    return SET;
  }
  else
  {
    return RESET;
  }
}

/** Tells the state of the control pin ON/OFF */
static FlagStatus ctrlpin_onoff_value(void)
{
  const uint32_t mask = (1 << CTRLPINNUM_ONOFF);
  if (GPIO_ReadValue(1) & mask)
  {
    return RESET;
  }
  else
  {
    return SET;
  }
}

// callback

/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Static_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Hall 1 edge detect callback function
* 					- Determines that the edge is meaningful (holds information)
* 					- If it is meaningful, gets the current timer value, resets the timer immediately
* 					  If the edge is meaninful, divides the value by 16 and rounds the value
*                    If the edge is rising, appends the value to the corresponding Hall 1 rising graph datagram
*                                if falling, to the Hall 1 falling graph datagram
*                    Sends the datagram via USART
*                  - If the edge is not meaningful, sets the enable flag to validate the next reading
*
* @param[in]	edge instance of edge_t
* @return 		None
**********************************************************************/
static void Hall1(edge_t edge)
{

  compute_next_Hall_edge(HALL1, edge);

  /* edge is meaningful */
  static FlagStatus enable = RESET;

  if (SET == enable)
  {
    const uint32_t pulse = Hall_timerValue();
    TIM_ResetCounter(LPC_TIM1); /* read and the reset immediately */
    uint32_t value = pulse;//(pulse >> 4) + ((pulse >> 3) & 1);
    if (value > DEFAULT_DURATION)
    {
      value = DEFAULT_DURATION;
    }



    Datagram datagram;

    append_duration(&hall12duration, value);
    x[((uint16_t)x_index)] = value;
    x_index++;
    if (x_index >= X_L)
    {
      x_index = 0;
    }



    if (EDGE_RISE == edge)
    {
      int16_t i = hall12duration.index - 1;
      if (i < 0)
      {
        i += DURATION_BUF_L;
      }
      datagram.size = 0; // reset datagram
      AppendText(&datagram, "f1", 2);
      AppendValue(&datagram, hall12duration.buf[i]);
      ext_stream__write(&datagram);


      //cycle_buffer(value);
      //cycle_lasthallvalues(value);

      // trimite datagrama cu diferenta
      const uint16_t d1 = diff1();

      datagram.size = 0; // reset datagram
      AppendText(&datagram, "f3", 2);
      AppendValue(&datagram, d1);
      //ext_stream__write(&datagram);

      //const uint16_t d2 = diff2();
      //datagram.size = 0; // reset datagram
      //AppendText(&datagram, "f4", 2);
      //AppendValue(&datagram, d2);
      //ext_stream__write(&datagram);
    }
    else
    {
      int16_t i = hall12duration.index - 1;
      if (i < 0)
      {
        i += DURATION_BUF_L;
      }
      datagram.size = 0; // reset datagram
      AppendText(&datagram, "f1", 2);
      AppendValue(&datagram, hall12duration.buf[i]);
      //ext_stream__write(&datagram);
      //cycle_buffer(value);
      //cycle_lasthallvalues(value);
    }

    //const uint32_t frequency = ((HALL_TIM_FREQ << 1) / pulse);
  }
  else
  {
    enable = SET;
  }


}

// callback

/* Public Functions ----------------------------------------------------------- */
/** @addtogroup Door_Static_Functions
* @{
*/

/*********************************************************************//**
* @brief 		Hall 2 edge detect callback function
* 					- Determines that the edge is meaningful (holds information)
* 					- If it is meaningful, gets the current timer value, resets the timer immediately
* 					  If the edge is meaninful, divides the value by 16 and rounds the value
*                    If the edge is rising, appends the value to the corresponding Hall 2 rising graph datagram
*                                if falling, to the Hall 2 falling graph datagram
*                    Sends the datagram via USART
*                  - If the edge is not meaningful, sets the enable flag to validate the next reading
*
* @param[in]	edge instance of edge_t
* @return 		None
**********************************************************************/
static void Hall2(edge_t edge)
{
  compute_next_Hall_edge(HALL2, edge);

  /* edge is meaningful */
  static FlagStatus enable = RESET;

  if (SET == enable)
  {
    const uint32_t pulse = Hall_timerValue();
    TIM_ResetCounter(LPC_TIM1); /* read and the reset immediately */
    uint32_t value = pulse;//(pulse >> 4) + ((pulse >> 3) & 1);


    if (value > DEFAULT_DURATION)
    {
      value = DEFAULT_DURATION;
    }

    Datagram datagram;

    append_duration(&hall12duration, value);

    x[((uint16_t)x_index)] = value;

    x_index++;

    if (x_index >= X_L)
    {
      x_index = 0;
    }

    //Datagram datagram;
   //eventual write


    if (EDGE_RISE == edge)
    {
      datagram.size = 0; // reset datagram
      AppendText(&datagram, "f1", 2);
      AppendValue(&datagram, value);
      //ext_stream__write(&datagram);
    }
    else
    {
      datagram.size = 0; // reset datagram
      AppendText(&datagram, "f1", 2);
      AppendValue(&datagram, value);
      //ext_stream__write(&datagram);
    }
  }
  else
  {
    enable = SET;
  }


}

static void motor_SM_process_input(void)
{
  MotorState nextMotorState = motorState;
  int32_t d;
  int16_t i, j;
  switch (motorState)
  {

  case MOTOR_STOP:


    if (SET == motorError)
    {
      if (RESET == key_value(KEY_UP) && RESET == key_value(KEY_DN))
      {
        motorError = RESET;
      }
      nextMotorState = MOTOR_STOP;
      reset_duration_buf();
      break;
    }
    if (SET == key_value(KEY_UP) || SET == key_value(KEY_DN))
    {
      //if (SET == direction) // directie UP
      {
        if (SET == key_value(KEY_UP)) // inaltime ok
        {
          ctrlpin_updn(SET); /* --> UP */
          if (counter_to_enable > 100)
          {
            //if (voltage >= 9 && voltage < 16)
            {
              nextMotorState = MOTOR_TO_RUN;
              addstate(MOTOR_TO_RUN);
              TIM_ResetCounter(LPC_TIM1);
              counter_to_run = 0;
            }
          }
        }
      }
      //      else
      //      {
      if (SET == key_value(KEY_DN)) // inaltime ok
      {
        ctrlpin_updn(RESET); /*  --> DOWN */
        if (counter_to_enable > 100)
        {
       //   if (voltage >= 9 && voltage < 16)
          {
            nextMotorState = MOTOR_TO_RUN;
            addstate(MOTOR_TO_RUN);
            TIM_ResetCounter(LPC_TIM1);
            counter_to_run = 0;
          }
        }
      }
      //      }

    }
    else
    {
      counter_to_enable = 0;
    }




    if (TIMEOUT_afisare < 500) // fortat = 1000.. initial a fost 250
    {
            if (SET == hall12duration.newEdge && RESET == hall12duration.lock)/**/
          {
           // buff_not_empty = SET;

          d = derivative(8); // se leaga de i (9 de sus)


          //  __enable_irq();
          j = (x_index - 1); //test din 4 in 4

          if (j < 0)
          {
            j += X_L;
          }

            Datagram datagram;
            datagram.size = 0; // reset datagram
            AppendText(&datagram, "f3", 2);
            AppendValue(&datagram, d);

            ext_stream__write(&datagram);

           hall12duration.newEdge = RESET;

        }

      break;
    }



   // counter_to_run = 0;
    TIMEOUT_motor_to_run_500ms = 0; // se judeca si in starile de eroare
    TIMEOUT_motor_to_stop_200ms = 0;
    buff_not_empty = RESET;
    //test_counter = 0;







    reset_duration_buf(); //test cu bufferul nou

   // TIM_ResetCounter(LPC_TIM1);



    break;


  case MOTOR_TO_RUN:
    // proprietate: detectie automata a lipsei feedback-ului la comanda motorului
    // defecte posibile: lipsa feedback senzori hall sau conexiune proasta comanda motor
    if (TIMEOUT_motor_to_run_500ms > 750) // fortat = 1000.. initial a fost 250
    {
      //asta trebuie sa fie mai mare decat ala de jos ultimu'

      nextMotorState = MOTOR_TO_STOP; //motor_stop initial
      error_motor_timeout_to_run = SET;
      motorError = SET;
      // flag pentru lipsa raspunsului de la motor
      TIMEOUT_motor_to_run_500ms = 0;
      TIMEOUT_motor_to_stop_200ms = 0;
      break;
    }
    i = x_index- 9;
    if (i < 0)
    {
      i += X_L;
    }

    /*--------------------------------*/
    if (SET == hall12duration.newEdge && RESET == hall12duration.lock && ((x[i] <= DEFAULT_DURATION || SET == buff_not_empty)))/**/
    {
      buff_not_empty = SET;
   //   __disable_irq();
      d = derivative(8); // se leaga de i (9 de sus)


    //  __enable_irq();
    j = (x_index - 1); //test din 4 in 4

    if (j < 0)
    {
      j += X_L;
    }

      Datagram datagram;
      datagram.size = 0; // reset datagram
      AppendText(&datagram, "f3", 2);
      AppendValue(&datagram, d);

      ext_stream__write(&datagram);

      hall12duration.newEdge = RESET;


      auxiliary_buffer[auxiliary_index]=d;
      auxiliary_index++;
      if (auxiliary_index==256) auxiliary_index=0;



      if (d < (-350)) // delta = 50 // - =acceleratie ... merge si +-
      {


        // in afara intervalului
        if (counter_to_run < 500) //ok  // PRAG 30 BUN IN CAZUL MOTORULUI NOSTRU
        { /// IMPORTANT>. PE ACEASTA VARIABILA SE VA FACE CALIBRAREA
          // LA 50 se blocheaza macaraua fara geam
          //ESTE INCA UN PRAG PENTRU DOWN
          //ACEST COUNTER_To run 80 trebuie sa fie mai mare decat cea care face trecerea in starea de RUn

          nextMotorState = MOTOR_TO_RUN; //a fost TO STOP
            addstate(MOTOR_TO_RUN);
        }
        else
        {
          nextMotorState = MOTOR_TO_STOP;  // TO_STOP
            addstate(MOTOR_TO_STOP);
          motorError = SET;
        }
      }
      else // in interval
      {
        if (counter_to_run > 400)  // 100
        {
          nextMotorState = MOTOR_RUN; // TO_STOP
            addstate(MOTOR_RUN);
            TIMEOUT_motor_run_state_4000ms = 0;
        }
        else
        {
          if (counter_to_run > 500)
          {
            nextMotorState = MOTOR_TO_RUN; // _TO_STOP
              addstate(MOTOR_TO_RUN);
          }
        }
      }
    }
   /* if (0 == toggle_dbgpin)
    {
      dbgpin(SET);
      toggle_dbgpin =1;
    }
    else
    {
      dbgpin(RESET);
      toggle_dbgpin = 0;
    }

    */

    break;
  case MOTOR_RUN:
     if (TIMEOUT_motor_run_state_4000ms > 4000) // fortat = 1000.. initial a fost 250
    {
      nextMotorState = MOTOR_TO_STOP; //motor_stop initial
      addstate(-999);
      error_motor_timeout_run_state = SET;
      motorError = SET;
      // flag pentru lipsa raspunsului de la motor
      TIMEOUT_motor_run_state_4000ms = 0;



      TIMEOUT_motor_to_stop_200ms = 0;
      break;
    }



    if (RESET == key_value(KEY_UP) && RESET == key_value(KEY_DN))
      {
        motorError = RESET;

        nextMotorState = MOTOR_TO_STOP;
        addstate(888);

        TIMEOUT_motor_to_stop_200ms = 0;
      }




    d = derivative(8);
    
    if (d<0) d=-d;


    if ((SET == key_value(KEY_UP) && d>15 ) || (SET == key_value(KEY_DN) && d>15) ) //forta trebui mai mare...
    //discutie dupa sens.. derivata mai sensibila inchidere
    {
      nextMotorState = MOTOR_STOP;
      motorError=SET;
      addstate(99);
      error_force_event=SET;

    //TIMEOUT_motor_to_stop_200ms = 0;
    }


             if (SET == hall12duration.newEdge && RESET == hall12duration.lock )/**/
            {
              buff_not_empty = SET;
           //   __disable_irq();
              d = derivative(8); // se leaga de i (9 de sus)


            //  __enable_irq();
            j = (x_index - 1); //test din 4 in 4

            if (j < 0)
            {
              j += X_L;
            }

              Datagram datagram;
              datagram.size = 0; // reset datagram
              AppendText(&datagram, "f3", 2);
              AppendValue(&datagram, d);

              ext_stream__write(&datagram);

              hall12duration.newEdge = RESET;
            }





    //counter_to_run = 0;
    break;
  case MOTOR_TO_STOP:

    if (TIMEOUT_motor_to_stop_200ms > 100) //fost 100
    {
      nextMotorState = MOTOR_STOP; //motor_stop initial

      error_motor_timeout_to_stop = SET;
      motorError = SET;
      // flag pentru lipsa raspunsului de la motor
      TIMEOUT_motor_to_stop_200ms = 0;

      TIMEOUT_afisare=0;
      break;
    }

    if (RESET == key_value(KEY_UP) && RESET == key_value(KEY_DN))
    {

      nextMotorState = MOTOR_STOP;
        addstate(MOTOR_STOP);
        TIMEOUT_afisare=0;

    }

    if (SET == hall12duration.newEdge && RESET == hall12duration.lock)/**/
    {
     // buff_not_empty = SET;

    d = derivative(8); // se leaga de i (9 de sus)


    //  __enable_irq();
    j = (x_index - 1); //test din 4 in 4

    if (j < 0)
    {
      j += X_L;
    }

      Datagram datagram;
      datagram.size = 0; // reset datagram
      AppendText(&datagram, "f3", 2);
      AppendValue(&datagram, d);

      ext_stream__write(&datagram);

     hall12duration.newEdge = RESET;

    }


   /* if (0 == toggle_dbgpin)
    {
      dbgpin(SET);
      toggle_dbgpin =1;
    }
    else
    {
      dbgpin(RESET);
      toggle_dbgpin = 0;
    }
*/

    break;
  case MOTOR_ERROR:

    break;
  case MOTOR_BLOCK:
    //nextMotorState = MOTOR_STOP;
    //direction = !direction;
    if (SET == direction) { direction = RESET; /* --> down */ }
    else { direction = SET; }
    break;
  default:
    //motorState = MOTOR_STOP;
    post_sw_fault("invalid state.");
  }

  motorState = nextMotorState;
}

static void motor_SM_set_output(void)
{
  switch (motorState)
  {
  case MOTOR_STOP:
    ctrlpin_onoff(RESET);
    break;
  case MOTOR_RUN:
    ctrlpin_onoff(SET);
    break;
  case MOTOR_TO_RUN:
    ctrlpin_onoff(SET);
    break;
  case MOTOR_TO_STOP:
    ctrlpin_onoff(RESET); //fost set
    break;
  case MOTOR_ERROR:
    ctrlpin_onoff(RESET);
    break;
  case MOTOR_BLOCK:
    ctrlpin_onoff(RESET);
    if (SET == direction) // UP
    {
      //direction = RESET; /* down */
    }
    break;
  default:
    motorState = MOTOR_STOP;
    post_sw_fault("invalid state.");
  }
}

void runMotorStateMachine(void)
{
  motor_SM_process_input();
  motor_SM_set_output();
}


